#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include<pcl/filters/passthrough.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=0;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

Eigen::Matrix4f P2_,T_camera_lidar_;

ros::Publisher black_odom_pub_,green_odom_pub_,white_odom_pub_;

void pcd2bin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, string& out_file)
{
    //Create & write .bin file
//    ofstream bin_file(out_file.c_str(),ios::out|ios::binary|ios::app);
    ofstream bin_file(out_file.c_str());
    if(!bin_file.good()) cout<<"Couldn't open "<<out_file<<endl;

    //PCD 2 BIN
    cout << "Converting "<<endl;
    for (size_t i = 0; i < cloud->points.size (); ++i)
    {
        bin_file.write((char*)&cloud->points[i].x,3*sizeof(float));
        bin_file.write((char*)&cloud->points[i].intensity,sizeof(float));
        //cout<< 	cloud->points[i]<<endl;
    }

    bin_file.close();
}

void saveCalib(string& out_file)
{
    ofstream outfile;      //终端输入--》内存--》文本
    outfile.open(out_file.c_str());//(输入流) （变量）（输出文件流）
    if(!outfile) cout<<"error"<<endl;         

    outfile<<"P0: "<<0<<" "<<0<<" "<<0<<" "<<0<<" "
    <<0<<" "<<0<<" "<<0<<" "<<0<<" "
     <<0<<" "<<0<<" "<<0<<" "<<0<<std::endl;
    outfile<<"P1: "<<0<<" "<<0<<" "<<0<<" "<<0<<" "
    <<0<<" "<<0<<" "<<0<<" "<<0<<" "
     <<0<<" "<<0<<" "<<0<<" "<<0<<std::endl;
    outfile<<"P2: "<<319.9988245765257<<" "<<0<<" "<<320.5<<" "<<0<<" "
    <<0<<" "<<319.9988245765257<<" "<<240.5<<" "<<0<<" "
     <<0<<" "<<0<<" "<<1<<" "<<0<<std::endl;
    outfile<<"P3: "<<319.9988245765257<<" "<<0<<" "<<320.5<<" "<<0<<" "
    <<0<<" "<<319.9988245765257<<" "<<240.5<<" "<<0<<" "
     <<0<<" "<<0<<" "<<1<<" "<<0<<std::endl;
    outfile<<"R0_rect: "<<1<<" "<<0<<" "<<0<<" "
    <<0<<" "<<1<<" "<<0<<" "
     <<0<<" "<<0<<" "<<1<<std::endl;
    outfile<<"Tr_velo_to_cam: "<<0<<" "<<-1<<" "<<0<<" "<<0.0<<" "
    <<0<<" "<<0<<" "<<-1<<" "<<0.0<<" "
     <<1<<" "<<0<<" "<<0<<" "<<0.0<<std::endl;
    outfile<<"Tr_imu_to_velo: "<<1<<" "<<0<<" "<<0<<" "<<0.23<<" "
    <<0<<" "<<1<<" "<<0<<" "<<0<<" "
     <<0<<" "<<0<<" "<<1<<" "<<-0.133<<std::endl;

    outfile.close();
}

vector<int> project2image2dbbox(double h, double w, double l, double x, double y, double z, double yaw){
    MatrixXd xyz(3,8);
    xyz<<l / 2., l / 2., -l / 2., -l / 2., l / 2., l / 2., -l / 2., -l / 2.,
            0,0,0,0,-h,-h,-h,-h,
            w / 2., -w / 2., -w / 2., w / 2., w / 2., -w / 2., -w / 2., w / 2.;
//    cout<<"hwl"<<h<<" "<<w<<" "<<l<<endl;
//    xyz<<-w/2, -w/2,w/2,w/2,-w/2, -w/2,w/2,w/2,
//            -h,0,0,-h,-h,0,0,-h,
//            l/2,l/2,l/2,l/2,-l/2,-l/2,-l/2,-l/2;

    MatrixXd rotM (3,3);
    rotM<<cos(yaw), 0,sin(yaw),
            0, 1,0,
            -sin(yaw), 0, cos(yaw);
//    rotM=rotM.transpose();
    xyz = rotM*xyz;
    xyz(0,0) = xyz(0,0) + x;xyz(0,1) = xyz(0,1)  + x; xyz(0,2) = xyz(0,2) + x; xyz(0,3) = xyz(0,3) + x;
    xyz(0,4) = xyz(0,4) + x;xyz(0,5) = xyz(0,5) + x;xyz(0,6) = xyz(0,6) + x;xyz(0,7) = xyz(0,7) + x;
    xyz(1,0) = xyz(1,0) + y; xyz(1,1) = xyz(1,1) + y; xyz(1,2) = xyz(1,2) + y; xyz(1,3) = xyz(1,3) + y;
    xyz(1,4) = xyz(1,4) + y;xyz(1,5) = xyz(1,5) + y;xyz(1,6) = xyz(1,6) + y;xyz(1,7) = xyz(1,7) + y;
    xyz(2,0) = xyz(2,0) + z; xyz(2,1) = xyz(2,1) + z; xyz(2,2) = xyz(2,2) + z; xyz(2,3) = xyz(2,3) + z;
    xyz(2,4) = xyz(2,4) + z;xyz(2,5) = xyz(2,5) + z;xyz(2,6) = xyz(2,6) + z;xyz(2,7) = xyz(2,7) + z;


    MatrixXd P2 (3,4);
    P2<<319.9988245765257, 0,320.5,0,
            0,319.9988245765257,240.5,0,
            0,0,1,0;

    MatrixXd xyz_new_M (4,8);
    xyz_new_M<<xyz(0,0), xyz(0,1),xyz(0,2),xyz(0,3),xyz(0,4),xyz(0,5),xyz(0,6),xyz(0,7),
            xyz(1,0), xyz(1,1),xyz(1,2),xyz(1,3),xyz(1,4),xyz(1,5),xyz(1,6),xyz(1,7),
            xyz(2,0), xyz(2,1),xyz(2,2),xyz(2,3),xyz(2,4),xyz(2,5),xyz(2,6),xyz(2,7),
            1,1,1,1,1,1,1,1;
//    cout<<"my debug bbox"<<xyz_new_M<<endl;
    xyz=P2*xyz_new_M;

    vector<double> x_image,y_image;
    for(int i=0; i<8; i++){
        x_image.push_back(xyz(0,i)/xyz(2,i));
        y_image.push_back(xyz(1,i)/xyz(2,i));
//        cout<<"x:"<<xyz(0,i)/xyz(2,i)<<endl;
//        cout<<"y:"<<xyz(1,i)/xyz(2,i)<<endl;
    }


    vector<int> bbox;
    bbox.push_back(int(*min_element(x_image.begin(),x_image.end())));
    bbox.push_back(int(*min_element(y_image.begin(),y_image.end())));
    bbox.push_back(int(*max_element(x_image.begin(),x_image.end())));
    bbox.push_back(int(*max_element(y_image.begin(),y_image.end())));
    if(bbox[0]<0)
        bbox[0]=0;
    if(bbox[1]<0)
        bbox[1]=0;
    if(bbox[2]>640)
        bbox[2]=640;
    if(bbox[3]>480)
        bbox[3]=480;
//    cout<<"my debug bbox"<<bbox[0]<<" "<<bbox[1]<<" "<<bbox[2]<<" "<<bbox[3]<<endl;
    return bbox;
}

void saveLabel(string& out_file, vector<vector<double>> label)
{
    ofstream outfile;      //终端输入--》内存--》文本
    outfile.open(out_file.c_str());//(输入流) （变量）（输出文件流）
    if(!outfile) cout<<"error"<<endl;         

    int object_num=label.size();
    for(int i=0; i<object_num; i++){
        outfile<<"Car "<<label[i][0]<<" "<<label[i][1]<<" "<<label[i][2]<<" "  //xyz
            <<label[i][3]<<" "<<label[i][4]<<" "<<label[i][5]<<" "  //rpy
            <<label[i][6]<<" "<<label[i][7]<<" "<<label[i][8]<<std::endl;   //lwh
    }
    outfile.close();
}

void saveImgLabel(string& out_file, vector<tf::Transform> label, string img_path, cv::Mat img)
{
    ofstream outfile;      //终端输入--》内存--》文本
    outfile.open(out_file.c_str(), ios::out|ios::binary|ios::app);//(输入流) （变量）（输出文件流）
    if(!outfile) cout<<"error"<<endl;         
    outfile<<img_path<<" ";

    int object_num=label.size();
    vector<int> bbox_image;
    int center_x, center_y;
    for(int i=0; i<object_num; i++){
        {
            Eigen::Vector3f pt;
            pt = (P2_*T_camera_lidar_*
                        Eigen::Vector4f(label[i].getOrigin().getX(), label[i].getOrigin().getY(), label[i].getOrigin().getZ(), 1)).topRows(3);
            int32_t u = static_cast<int32_t>(pt(0)/pt(2));
            int32_t v = static_cast<int32_t>(pt(1)/pt(2));
            // cout<<"center x y "<<u<<" "<<v<<endl;
            if (u>=0 && u< 640 && v >= 0 && v < 480 && pt(2)>0 ){
                center_x = u;
                center_y = v;
            }else{
                continue;
            }
        }
        if(i == 0)
            circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(255,0, 0), 4);
        if(i == 1)
            circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(0,255, 0), 4);
        if(i == 2)
            circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(255,255, 0), 4);

        outfile<<center_x<<","<<center_y<<","<<i<<" ";
    }
    outfile<<endl;
    outfile.close();
    cv::imshow("img_center", img);
    cv::waitKey(1);
}

void saveTrajectory(string& out_file,double time_stamp, double x,double y,double z,double ow,double ox,double oy,double oz, double vx,double vy,double vz)
{
    ofstream outfile(out_file.c_str(),ios::out|ios::binary|ios::app);
    if(!outfile) cout<<"error"<<endl;
    outfile<<time_stamp<<" "<<x<<" "<<y<<" "<<z<<" "    //xyz
           <<ow<<" "<<ox<<" "<<oy<<" "<<oz<<" "<<vx<<" "<<vy<<" "<<vz<<std::endl;  //
    outfile.close();
}

bool judgeLivoxRange(const PointType& pt){
    double ratio = sin(0.707);
    if(pt.y-ratio*pt.x -3< 0 && pt.y+ratio*pt.x +3>0 ){
        return true;
    }else{
        return false;
    }
}

void cloudCB(const sensor_msgs::PointCloud2ConstPtr& pc_msg, const sensor_msgs::ImageConstPtr& front_image_msg,const nav_msgs::OdometryConstPtr& master_traj_gt_msg, const nav_msgs::OdometryConstPtr& black_traj_gt_msg,
             const nav_msgs::OdometryConstPtr& green_traj_gt_msg){
    ROS_INFO("cb ok");
    frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                      "%06d", frame_num_));
    pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
    pcl::fromROSMsg(*pc_msg, *pc_tmp);

    tf::Transform T_livox,T_black,T_green,T_white,T_livox2black,T_livox2green,T_livox2white,T_lidar2base,T_cam2lidar,center_temp,T_obect,T_plane_livox_tf,
                                T_plane_black,T_plane_green,T_plane_white,T_c_plane_plane;
    T_livox.setOrigin(tf::Vector3(master_traj_gt_msg->pose.pose.position.x, master_traj_gt_msg->pose.pose.position.y, master_traj_gt_msg->pose.pose.position.z));
    T_livox.setRotation(tf::Quaternion(master_traj_gt_msg->pose.pose.orientation.x,master_traj_gt_msg->pose.pose.orientation.y,
                                     master_traj_gt_msg->pose.pose.orientation.z,master_traj_gt_msg->pose.pose.orientation.w));
    T_black.setOrigin(tf::Vector3(black_traj_gt_msg->pose.pose.position.x, black_traj_gt_msg->pose.pose.position.y, black_traj_gt_msg->pose.pose.position.z));
    T_black.setRotation(tf::Quaternion(black_traj_gt_msg->pose.pose.orientation.x,black_traj_gt_msg->pose.pose.orientation.y,
                                       black_traj_gt_msg->pose.pose.orientation.z,black_traj_gt_msg->pose.pose.orientation.w));
    T_green.setOrigin(tf::Vector3(green_traj_gt_msg->pose.pose.position.x, green_traj_gt_msg->pose.pose.position.y, green_traj_gt_msg->pose.pose.position.z));
    T_green.setRotation(tf::Quaternion(green_traj_gt_msg->pose.pose.orientation.x,green_traj_gt_msg->pose.pose.orientation.y,
                                       green_traj_gt_msg->pose.pose.orientation.z,green_traj_gt_msg->pose.pose.orientation.w));
    // T_white.setOrigin(tf::Vector3(white_traj_gt_msg->pose.pose.position.x, white_traj_gt_msg->pose.pose.position.y, white_traj_gt_msg->pose.pose.position.z));
    // T_white.setRotation(tf::Quaternion(white_traj_gt_msg->pose.pose.orientation.x,white_traj_gt_msg->pose.pose.orientation.y,
    //                                    white_traj_gt_msg->pose.pose.orientation.z,white_traj_gt_msg->pose.pose.orientation.w));

    tf::Transform t_temp;
    t_temp.setOrigin(tf::Vector3(0,0,1.094));
    t_temp.setRotation(tf::Quaternion(0,0,0));
    T_livox = T_livox*t_temp;

    t_temp.setOrigin(tf::Vector3(0,0,0.425));
    T_black = T_black*t_temp;
    T_green = T_green*t_temp;
    // T_white = T_white*t_temp;

    T_livox2black=T_livox.inverse()*T_black;
    T_livox2green=T_livox.inverse()*T_green;
    // T_livox2white=T_livox.inverse()*T_white;

    //pub odom
    nav_msgs::Odometry tracker_odom_cur_msg;
    tracker_odom_cur_msg.header.frame_id = "map";
    tracker_odom_cur_msg.child_frame_id = "black";
    tracker_odom_cur_msg.header.stamp = ros::Time::now();
    geometry_msgs::Pose pose_msg_temp;
    tf::poseTFToMsg(T_livox2black, pose_msg_temp);
    tracker_odom_cur_msg.pose.pose = pose_msg_temp;
    tracker_odom_cur_msg.twist.twist.linear.x = 0;
    tracker_odom_cur_msg.twist.twist.linear.y = 0;
    tracker_odom_cur_msg.twist.twist.linear.z = 0;
    black_odom_pub_.publish(tracker_odom_cur_msg);

    tracker_odom_cur_msg.child_frame_id = "green";
    tracker_odom_cur_msg.header.stamp = ros::Time::now();
    tf::poseTFToMsg(T_livox2green, pose_msg_temp);
    tracker_odom_cur_msg.pose.pose = pose_msg_temp;
    green_odom_pub_.publish(tracker_odom_cur_msg);

    // tracker_odom_cur_msg.child_frame_id = "white";
    // tracker_odom_cur_msg.header.stamp = ros::Time::now();
    // tf::poseTFToMsg(T_livox2white, pose_msg_temp);
    // tracker_odom_cur_msg.pose.pose = pose_msg_temp;
    // white_odom_pub_.publish(tracker_odom_cur_msg);

    pcl::PassThrough<PointType> pass;
    pass.setInputCloud (pc_tmp);//这个参数得是指针，类对象不行
    pass.setFilterFieldName ("x");//设置想在哪个坐标轴上操作
    pass.setFilterLimits (0, 30);//将x轴的0到1范围内
    pass.setFilterLimitsNegative (false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter (*pc_tmp);//输出到结果指针
    pass.setInputCloud (pc_tmp);//这个参数得是指针，类对象不行
    pass.setFilterFieldName ("y");//设置想在哪个坐标轴上操作
    pass.setFilterLimits (-20, 20);//将x轴的0到1范围内
    pass.setFilterLimitsNegative (false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter (*pc_tmp);//输出到结果指针
    // pass.setInputCloud (pc_tmp);//这个参数得是指针，类对象不行
    // pass.setFilterFieldName ("z");//设置想在哪个坐标轴上操作
    // pass.setFilterLimits (-3.5, 1.5);//将x轴的0到1范围内
    // pass.setFilterLimitsNegative (false);//保留（true就是删除，false就是保留而删除此区间外的）
    // pass.filter (*pc_tmp);//输出到结果指针
    pcl::PointCloud<PointType>::Ptr p_det(new pcl::PointCloud<PointType>);
    for (uint i = 1; i < pc_tmp->points.size(); i++)
    {
        if ((pc_tmp->points[i].x>0&&pc_tmp->points[i].x<20) && 
            (pc_tmp->points[i].y>-10&&pc_tmp->points[i].y<10)&&
            (pc_tmp->points[i].z>-5&&pc_tmp->points[i].z<5)&&
            judgeLivoxRange(pc_tmp->points[i]))
        {
            p_det->push_back(pc_tmp->points[i]);
        }
    }
   cout<<"pc points size "<<p_det->size()<<endl;

    sensor_msgs::PointCloud2 pc_plane_display;
    pcl::toROSMsg(*p_det, pc_plane_display);
    pc_plane_display.header.frame_id = "map";
    cloud_pub.publish(pc_plane_display);

    double length = 0.75,width = 0.5,height = 0.85;
    vector<vector<double>> label;
    {
        vector<double> label_this(9);
        label_this[0]=T_livox2black.getOrigin().x();label_this[1]=T_livox2black.getOrigin().y();label_this[2]=T_livox2black.getOrigin().z();
        double r,p,yaw;
        tf::Matrix3x3(T_livox2black.getRotation()).getRPY(r,p,yaw);
        label_this[3] = r;
        label_this[4] = p;
        label_this[5] = yaw;
        label_this[6] = length;
        label_this[7] = width;
        label_this[8] = height;
        label.push_back(label_this);
    }

    {
        vector<double> label_this(9);
        label_this[0]=T_livox2green.getOrigin().x();label_this[1]=T_livox2green.getOrigin().y();label_this[2]=T_livox2green.getOrigin().z();
        double r,p,yaw;
        tf::Matrix3x3(T_livox2black.getRotation()).getRPY(r,p,yaw);
        label_this[3] = r;
        label_this[4] = p;
        label_this[5] = yaw;
        label_this[6] = length;
        label_this[7] = width;
        label_this[8] = height;
        label.push_back(label_this);
    }

    // {
    //     vector<double> label_this(3);
    //     label_this[0]=T_livox2white.getOrigin().x();label_this[1]=T_livox2white.getOrigin().y();label_this[2]=T_livox2white.getOrigin().z();
    //     label.push_back(label_this);
    // }

    string tranin_or_test="training";
    //   string tranin_or_test="testing";
    string dataset_name = "0813";
    cout<<tranin_or_test<<endl;
    cout<<frame_id_string_<<endl;
   pc_path_name_="/media/st/data_fast/gazebo/"+dataset_name+"/"+tranin_or_test+"/label_2/"+frame_id_string_+".txt";
   saveLabel(pc_path_name_,label);

   pc_path_name_="/media/st/data_fast/gazebo/"+dataset_name+"/"+tranin_or_test+"/velodyne/"+frame_id_string_+".bin";
   pcd2bin(p_det,pc_path_name_);

   cv_bridge::CvImagePtr image_front;
   image_front = cv_bridge::toCvCopy(front_image_msg, sensor_msgs::image_encodings::BGR8);
   cv::imwrite("/media/st/data_fast/gazebo/"+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png",(*image_front).image);
   string img_center_path_name_;
   vector<tf::Transform> center_lidar_v = {T_livox2black,T_livox2green};
   img_center_path_name_="/media/st/data_fast/gazebo/"+dataset_name+"/"+tranin_or_test+"/"+"img_center_label.txt";
    saveImgLabel(img_center_path_name_, center_lidar_v, "/media/st/data_fast/gazebo/"+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png", 
                                   (*image_front).image );

//    pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/calib/"+frame_id_string_+".txt";
//    saveCalib(pc_path_name_);

    pc_path_name_="/media/st/data_fast/gazebo/"+dataset_name+"/"+tranin_or_test+"/trajectory/master.txt";
    saveTrajectory(pc_path_name_, master_traj_gt_msg->header.stamp.toSec(),
            master_traj_gt_msg->pose.pose.position.x,master_traj_gt_msg->pose.pose.position.y,master_traj_gt_msg->pose.pose.position.z,
                                master_traj_gt_msg->pose.pose.orientation.w,master_traj_gt_msg->pose.pose.orientation.x,
                                master_traj_gt_msg->pose.pose.orientation.y,master_traj_gt_msg->pose.pose.orientation.z,
                   master_traj_gt_msg->twist.twist.linear.x,master_traj_gt_msg->twist.twist.linear.y,master_traj_gt_msg->twist.twist.linear.z);

    pc_path_name_="/media/st/data_fast/gazebo/"+dataset_name+"/"+tranin_or_test+"/trajectory/black.txt";
    saveTrajectory(pc_path_name_,black_traj_gt_msg->header.stamp.toSec(),
                   black_traj_gt_msg->pose.pose.position.x,black_traj_gt_msg->pose.pose.position.y,black_traj_gt_msg->pose.pose.position.z,
                   black_traj_gt_msg->pose.pose.orientation.w,black_traj_gt_msg->pose.pose.orientation.x,
                   black_traj_gt_msg->pose.pose.orientation.y,black_traj_gt_msg->pose.pose.orientation.z,
                   black_traj_gt_msg->twist.twist.linear.x,black_traj_gt_msg->twist.twist.linear.y,black_traj_gt_msg->twist.twist.linear.z);

    pc_path_name_="/media/st/data_fast/gazebo/"+dataset_name+"/"+tranin_or_test+"/trajectory/green.txt";
    saveTrajectory(pc_path_name_,green_traj_gt_msg->header.stamp.toSec(),
                   green_traj_gt_msg->pose.pose.position.x,green_traj_gt_msg->pose.pose.position.y,green_traj_gt_msg->pose.pose.position.z,
                   green_traj_gt_msg->pose.pose.orientation.w,green_traj_gt_msg->pose.pose.orientation.x,
                   green_traj_gt_msg->pose.pose.orientation.y,green_traj_gt_msg->pose.pose.orientation.z,
                   green_traj_gt_msg->twist.twist.linear.x,green_traj_gt_msg->twist.twist.linear.y,green_traj_gt_msg->twist.twist.linear.z);


    // pc_path_name_="/media/st/data_fast/gazebo/"+dataset_name+"/"+tranin_or_test+"/trajectory/white.txt";
    // saveTrajectory(pc_path_name_,white_traj_gt_msg->header.stamp.toSec(),
    //                white_traj_gt_msg->pose.pose.position.x,white_traj_gt_msg->pose.pose.position.y,white_traj_gt_msg->pose.pose.position.z,
    //                white_traj_gt_msg->pose.pose.orientation.w,white_traj_gt_msg->pose.pose.orientation.x,
    //                white_traj_gt_msg->pose.pose.orientation.y,white_traj_gt_msg->pose.pose.orientation.z,
    //                white_traj_gt_msg->twist.twist.linear.x,white_traj_gt_msg->twist.twist.linear.y,white_traj_gt_msg->twist.twist.linear.z);

    frame_num_++;
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;
    
   cloud_pub=nh.advertise<sensor_msgs::PointCloud2>("/make_label/pc",10,true);
    black_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/make_label/black_odom", 1);
    green_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/make_label/green_odom", 1);
    white_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/make_label/white_odom", 1);

    tf::Transform T_cam2lidar;
    T_cam2lidar.setOrigin(tf::Vector3(0.02, -0.01,-0.07));
    T_cam2lidar.setRotation(tf::Quaternion(M_PI/2,0,-M_PI/2));
    T_cam2lidar=T_cam2lidar.inverse();
    T_camera_lidar_ = Eigen::Matrix4f::Identity();
    Eigen::Quaternionf q_temp(T_cam2lidar.getRotation().getW(),T_cam2lidar.getRotation().getX(),
                                        T_cam2lidar.getRotation().getY(),T_cam2lidar.getRotation().getZ());
    T_camera_lidar_.block<3,3>(0,0)= q_temp.toRotationMatrix();
    T_camera_lidar_(0,3) = T_cam2lidar.getOrigin().getX();
    T_camera_lidar_(1,3) = T_cam2lidar.getOrigin().getY();
    T_camera_lidar_(2,3) = T_cam2lidar.getOrigin().getZ();

    P2_ = Eigen::Matrix4f::Identity();
    float p2[12] = {319.9988245765257, 0, 320.5, 0,
                                 0, 319.9988245765257, 240.5, 0, 
                                 0, 0, 1, 0};
    for (int i = 0; i<12; i++){
        P2_(i/4, i%4) = p2[i];
    }


    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "/robot1/Ruby80", 1);
    message_filters::Subscriber<sensor_msgs::Image> image_front_sub(nh, "/robot1/front_camera/image_raw", 1);
    message_filters::Subscriber<nav_msgs::Odometry> master_traj_sub(nh, "/robot1/base_pose_ground_truth", 1);
    message_filters::Subscriber<nav_msgs::Odometry> black_traj_sub(nh, "/nexus3/base_pose_ground_truth", 1);
    message_filters::Subscriber<nav_msgs::Odometry> green_traj_sub(nh, "/nexus0/base_pose_ground_truth", 1);
    // message_filters::Subscriber<nav_msgs::Odometry> white_traj_sub(nh, "/nexus2/base_pose_ground_truth", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sensor_msgs::Image, nav_msgs::Odometry,nav_msgs::Odometry,nav_msgs::Odometry> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(20), pc_sub, image_front_sub, master_traj_sub,black_traj_sub, green_traj_sub);
    sync.setMaxIntervalDuration(ros::Duration(0.5));
    sync.registerCallback(boost::bind(&cloudCB, _1, _2,_3,_4,_5));
    ros::spin();

    return 0;
}
